//------------------------------------------------------------------------------
//
//  Hemisson Service
//
//  Written Dec-2007 by Trevor Taylor, Software Technology, Australia
//
//  This service is for the Hemisson robot from K-Team. It uses the Generic
//  Brick contract. It demonstrates most of the functions of the Generic Brick
//  because a Hemisson has 8 IR sensors, 4 switches, 4 LEDs, a Buzzer and two
//  motors (differential drive).
//
//  A generic differential drive service and a bumper (contact sensor array)
//  service are also built into the same DLL.
//
//------------------------------------------------------------------------------

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using System;
using System.Collections.Generic;
using W3C.Soap;
// Shortened the aliases
//using dssphttp = Microsoft.Dss.Core.DsspHttp;
using drive = Microsoft.Robotics.Services.Drive.Proxy;
using brick = ProMRDS.Robotics.GenericBrick.Proxy;
using xml = System.Xml;


// Added the following after creating the service:
using submgr = Microsoft.Dss.Services.SubscriptionManager;
using System.ComponentModel;

using ProMRDS.Robotics.HemissonComms;

// For HTTP stuff
using System.Net;
using Microsoft.Dss.Core.DsspHttp;

using util = ProMRDS.Robotics.GenericBrickUtil.Util;


namespace ProMRDS.Robotics.Hemisson
{
    /// <summary>
    /// Hemisson Service
    /// </summary>
    // The control code uses a polling loop that blocks a thread
    // The ActivationSettings attribute with Sharing == false makes the runtime
    // dedicate a dispatcher thread pool just for this service.
    [ActivationSettings(ShareDispatcher = false, ExecutionUnitsPerDispatcher = 4)]
    [Contract(Contract.Identifier)]
    [AlternateContract("http://www.promrds.com/contracts/2008/01/genericbrick.html")]
    [DisplayName("Hemisson Robot")]
    [Description("Provides access to the Hemisson robot")]
    public class HemissonService : DsspServiceBase
    {
        /// <summary>
        /// Hemisson State
        /// </summary>
        // Add an initial state partner
        [InitialStatePartner(Optional = true, ServiceUri = "Hemisson.Config.xml")]
        private brick.GenericBrickState _state = new brick.GenericBrickState();
        
        /// <summary>
        /// Hemisson Main Operations Port
        /// </summary>
        [ServicePort("/hemisson", AllowMultipleInstances=false)]
        private brick.GenericBrickOperations _mainPort = new brick.GenericBrickOperations();

        #region Globals

        // Add an XSLT file to display the state nicely
        [EmbeddedResource("ProMRDS.Robotics.Hemisson.Resources.Hemisson.xslt")]
        string _transform = null;

        // Subscription Manager Partner
        [Partner("SubMgr", Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)]
        private submgr.SubscriptionManagerPort _subMgrPort = new submgr.SubscriptionManagerPort();

        // Comms control
        private HemissonControl _control = null;

        // Minimum polling interval (in millisec) that is acceptable due to bandwidth, etc.
        private const int _minimumPollingInterval = 50;

        // Change this value for your setup or edit the config file
        private const int DefaultSerialPort = 7;

        private bool _shutdown = false;

        // Internal information about the LEDs on the robot
        private int[] LEDIds;
        private int LEDMask;

        // NOTE: The order of the values in this array is very important!
        // They must match the order in the Fast Binary Read command.
        private int[] IRIds;

        private int[] SwitchIds;


        #endregion

        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public HemissonService(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
        }

        #region Initialization

        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
            InitializeState();

            base.Start();

            SpawnIterator(StartRobot);

        }

        private IEnumerator<ITask> StartRobot()
        {
            // Create a new Control instance
            // This connects to the serial port
            _control = new HemissonControl(_mainPort);

            // Try to connect to the robot
            if (ConnectToRobot())
            {
                // Kick off polling
                SetPollTimer();

                // Give the polling time to get the current state
                yield return Arbiter.Receive(false,
                    TimeoutPort(2*Math.Abs(_state.Configuration.PollingInterval)),
                    delegate(DateTime dt) { }
                );

                // Display the Switches
                //_control.PollSwitches();
                LogInfo(LogGroups.Console, "Switches: " + _state.Switches);

            }
            else
                LogError(LogGroups.Console, "Connection to Hemisson failed");

            // Save the state now because it contains the Firmware info.
            // Even if the robot failed to start, we might need to create an
            // empty config file for the user to edit.
            SaveState(_state);
        }

        /// <summary>
        /// Validate and Initialize State
        /// </summary>
        private void InitializeState()
        {
            if (_state == null)
            {
                _state = new brick.GenericBrickState();
                _state.Connected = false;
            }

            _state.Name = "Hemisson Jan-08 V1.0";

            // Flag all of the capabilities of this brick
            // NOTE: The DriveDistance and RotateDegrees operations are not
            // implemented here -- they are in the HemissonDrive service.
            _state.Capabilities =
                            brick.BrickCapabilities.CanControlMotors |
//                            brick.BrickCapabilities.CanDriveDistance |
//                            brick.BrickCapabilities.CanRotateDegrees |
                            brick.BrickCapabilities.HasAnalogIn |
                            brick.BrickCapabilities.HasDigitalIn |
                            brick.BrickCapabilities.HasDigitalOut |
                            brick.BrickCapabilities.HasBumpers |
                            brick.BrickCapabilities.HasBuzzer |
                            brick.BrickCapabilities.HasDifferentialDrive |
                            brick.BrickCapabilities.HasLEDs |
                            brick.BrickCapabilities.HasLineFollowing |
                            brick.BrickCapabilities.HasSwitches;

            // Reset these in case they were in the saved state
            _state.Connected = false;

            // Always reinitialize the Firmware or it might carry forward
            // the wrong information
            _state.Firmware = new brick.Firmware();

            if (_state.CommsStats == null)
                _state.CommsStats = new brick.CommsStats();

            _state.CommsStats.MessagesSent = 0;
            _state.CommsStats.MessagesReceived = 0;
            _state.CommsStats.ErrorCount = 0;

            if (_state.Configuration == null)
            {
                _state.Configuration = new brick.CommsConfig();
                _state.Configuration.SerialPort = DefaultSerialPort;
                _state.Configuration.Delay = 50;
                _state.Configuration.PollingInterval = _minimumPollingInterval;
                _state.Configuration.Timeout = 250;
            }
            // If PollingInterval is negative, then no polling will occur.
            // In this case, any partner must explicity read the sensors
            // by Get messages.
            if (_state.Configuration.PollingInterval >= 0 &&
                _state.Configuration.PollingInterval < _minimumPollingInterval)
                _state.Configuration.PollingInterval = _minimumPollingInterval;

            // Always initialize the Sensors just in case the
            // config file has been tampered with --
            // we don't support reconfiguration anyway
            InitializeSensors();


            // Always initialize the Actuators
            InitializeActuators();

            // Set up an array for easy access to the LEDs.
            // NOTE: There is no requirement for the LEDs to be in any
            // particular order. They should be in some logical order
            // that the user can understand, but this is not necessarily
            // the same as the numeric order of the Pins (or Hardware IDs).
            // The order in the array determines the bit positions in the
            // LEDs state property. This is a subtle relationship.
            LEDIds = new int[4];
            LEDIds[0] = (int)HwIds.LED1;
            LEDIds[1] = (int)HwIds.LED2;
            LEDIds[2] = (int)HwIds.LED3;
            LEDIds[3] = (int)HwIds.LED4;
            LEDMask = 0x0F;

            SwitchIds = new int[4];
            SwitchIds[0] = (int)HwIds.Switch1;
            SwitchIds[1] = (int)HwIds.Switch2;
            SwitchIds[2] = (int)HwIds.Switch3;
            SwitchIds[3] = (int)HwIds.Switch4;

            IRIds = new int[8];
            IRIds[0] = (int)HwIds.IRFront;
            IRIds[1] = (int)HwIds.IRFrontRight;
            IRIds[2] = (int)HwIds.IRFrontLeft;
            IRIds[3] = (int)HwIds.IRRight;
            IRIds[4] = (int)HwIds.IRLeft;
            IRIds[5] = (int)HwIds.IRRear;
            IRIds[6] = (int)HwIds.IRGroundRight;
            IRIds[7] = (int)HwIds.IRGroundLeft;

            if (_state.DrivePower == null)
                _state.DrivePower = new brick.DrivePower();

            // Initialize the Motors to stopped
            _state.DrivePower.LeftPower = 0;
            _state.DrivePower.RightPower = 0;

        }

        void InitializeSensors()
        {
            // Create a new Sensors list
            _state.Sensors = new List<brick.Device>();

            // Add the Sensor devices
            // NOTE: The order is not important, although it makes sense to put
            // the most frequently accessed sensors first.

            // Create the Analog Inputs --
            // Currently there are none but two input pins can be 
            // configured as ADCs if necessary: Pins 0 and 1

            // Create the Digital Inputs
            // NOTE: The inputs on the Hemisson are not addressed
            // by pin number, so the HW Id is used instead
            _state.Sensors.Add(new brick.Device(
                "IR Front Left",
                (int)HwIds.IRFrontLeft,
                brick.DeviceTypes.AnalogIn,
                brick.DeviceFunctions.IRRangeSensor,
                brick.Location.FrontLeft,
                0,
                (int)HwIds.IRFrontLeft,
                DateTime.Now,
                false,
                0,
                0,
                255
                )
            );
            _state.Sensors.Add(new brick.Device(
                "IR Front",
                (int)HwIds.IRFront,
                brick.DeviceTypes.AnalogIn,
                brick.DeviceFunctions.IRRangeSensor,
                brick.Location.FrontCenter,
                0,
                (int)HwIds.IRFront,
                DateTime.Now,
                false,
                0,
                0,
                255
                )
            );
            _state.Sensors.Add(new brick.Device(
                "IR Front Right",
                (int)HwIds.IRFrontRight,
                brick.DeviceTypes.AnalogIn,
                brick.DeviceFunctions.IRRangeSensor,
                brick.Location.FrontRight,
                0,
                (int)HwIds.IRFrontRight,
                DateTime.Now,
                false,
                0,
                0,
                255
                )
            );
            _state.Sensors.Add(new brick.Device(
                "IR Side Left",
                (int)HwIds.IRLeft,
                brick.DeviceTypes.AnalogIn,
                brick.DeviceFunctions.IRRangeSensor,
                brick.Location.SideLeft,
                0,
                (int)HwIds.IRLeft,
                DateTime.Now,
                false,
                0,
                0,
                255
                )
            );
            _state.Sensors.Add(new brick.Device(
                "IR Side Right",
                (int)HwIds.IRRight,
                brick.DeviceTypes.AnalogIn,
                brick.DeviceFunctions.IRRangeSensor,
                brick.Location.SideRight,
                0,
                (int)HwIds.IRRight,
                DateTime.Now,
                false,
                0,
                0,
                255
                )
            );
            _state.Sensors.Add(new brick.Device(
                "IR Rear",
                (int)HwIds.IRRear,
                brick.DeviceTypes.AnalogIn,
                brick.DeviceFunctions.IRRangeSensor,
                brick.Location.RearCenter,
                0,
                (int)HwIds.IRRear,
                DateTime.Now,
                false,
                0,
                0,
                255
                )
            );
            _state.Sensors.Add(new brick.Device(
                "IR Ground Front Left",
                (int)HwIds.IRGroundLeft,
                brick.DeviceTypes.AnalogIn,
                brick.DeviceFunctions.LineFollower,
                brick.Location.GroundFrontLeft,
                0,
                (int)HwIds.IRGroundLeft,
                DateTime.Now,
                false,
                0,
                0,
                255
                )
            );
            _state.Sensors.Add(new brick.Device(
                "IR Ground Front Right",
                (int)HwIds.IRGroundRight,
                brick.DeviceTypes.AnalogIn,
                brick.DeviceFunctions.LineFollower,
                brick.Location.GroundFrontRight,
                0,
                (int)HwIds.IRGroundRight,
                DateTime.Now,
                false,
                0,
                0,
                255
                )
            );

            // Switches
            _state.Sensors.Add(new brick.Device(
                "Switch 1",
                (int)HwIds.Switch1,
                brick.DeviceTypes.DigitalIn,
                brick.DeviceFunctions.Switch,
                brick.Location.SideLeft,
                0,
                (int)HwIds.Switch1,
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );
            _state.Sensors.Add(new brick.Device(
                "Switch 2",
                (int)HwIds.Switch2,
                brick.DeviceTypes.DigitalIn,
                brick.DeviceFunctions.Switch,
                brick.Location.SideLeft,
                0,
                (int)HwIds.Switch2,
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );
            _state.Sensors.Add(new brick.Device(
                "Switch 3",
                (int)HwIds.Switch3,
                brick.DeviceTypes.DigitalIn,
                brick.DeviceFunctions.Switch,
                brick.Location.SideLeft,
                0,
                (int)HwIds.Switch3,
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );
            _state.Sensors.Add(new brick.Device(
                "Switch 4",
                (int)HwIds.Switch4,
                brick.DeviceTypes.DigitalIn,
                brick.DeviceFunctions.Switch,
                brick.Location.SideLeft,
                0,
                (int)HwIds.Switch4,
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );
        }

        void InitializeActuators()
        {
            // Create a new Actuators list
            _state.Actuators = new List<brick.Device>();

            // Add the Actuator devices
            // NOTE: The order is not important, although it makes sense to put
            // the most frequently accessed actuators first.

            // There are no Analog Outputs

            // Digital Outputs
            _state.Actuators.Add(new brick.Device(
                "LED 1",
                (int)HwIds.LED1,
                brick.DeviceTypes.DigitalOut,
                brick.DeviceFunctions.LED,
                brick.Location.RearFarLeft,
                0,
                (int)HwIds.LED1,         // LED1 cannot be used because it blinks
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );
            _state.Actuators.Add(new brick.Device(
                "LED 2",
                (int)HwIds.LED2,
                brick.DeviceTypes.DigitalOut,
                brick.DeviceFunctions.LED,
                brick.Location.FrontLeft,
                0,
                (int)HwIds.LED2,
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );
            _state.Actuators.Add(new brick.Device(
                "LED 3",
                (int)HwIds.LED3,
                brick.DeviceTypes.DigitalOut,
                brick.DeviceFunctions.LED,
                brick.Location.FrontRight,
                0,
                (int)HwIds.LED3,
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );
            // This one is red and at the back of the robot
            _state.Actuators.Add(new brick.Device(
                "LED 4",
                (int)HwIds.LED4,
                brick.DeviceTypes.DigitalOut,
                brick.DeviceFunctions.LED,
                brick.Location.RearFarRight,
                0,
                (int)HwIds.LED4,
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );
            _state.Actuators.Add(new brick.Device(
                "Buzzer",
                (int)HwIds.Buzzer,
                brick.DeviceTypes.DigitalOut,
                brick.DeviceFunctions.Buzzer,
                brick.Location.Other,
                0,
                (int)HwIds.Buzzer,
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );

        }

        #endregion

        #region Operation Handlers

        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(brick.Get get)
        {
            // Make sure the comms stats are up to date
            _state.CommsStats.MessagesSent = _control.MessagesSentCounter;
            _state.CommsStats.ErrorCount = _control.ErrorCounter;

            get.ResponsePort.Post(_state);
            yield break;
        }
        
        /// <summary>
        /// HttpGet Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpGetHandler(HttpGet get)
        {
            // Make sure the comms stats are up to date
            _state.CommsStats.MessagesSent = _control.MessagesSentCounter;
            _state.CommsStats.ErrorCount = _control.ErrorCounter;

            get.ResponsePort.Post(new HttpResponseType(
                HttpStatusCode.OK,
                _state,
                _transform)
            );
            yield break;
        }
        
        /// <summary>
        /// HttpPost Handler
        /// </summary>
        /// <param name="submit"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpPostHandler(HttpPost submit)
        {
            // TODO: Implement Submit operations here.
            throw new NotImplementedException("HttpPost is not implemented");
        }
        
        /// <summary>
        /// Replace Handler
        /// </summary>
        /// <param name="replace"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> ReplaceHandler(brick.Replace replace)
        {
            _state = replace.Body;
            replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
            yield break;
        }
        
        /// <summary>
        /// Subscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SubscribeHandler(brick.Subscribe subscribe)
        {
            yield return Arbiter.Choice(
                SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
                delegate(SuccessResult success) { },
                delegate(Exception ex)
                {
                    LogError(ex);
                    throw ex;
                }
            );

//            SendNotification<brick.QueryDrivePower>(_subMgrPort, subscribe.Body.Subscriber, new brick.QueryDrivePower(_state.DrivePower));
//            SendNotification<brick.UpdateSensors>(_subMgrPort, subscribe.Body.Subscriber, new brick.UpdateSensors(new brick.UpdateSensorsRequest(_state.Sensors)));

            yield break;
        }
        
        /// <summary>
        /// ConfigureBrick Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> ConfigureBrickHandler(brick.ConfigureBrick update)
        {
            // Configuration options are very limited!
            if (update.Body.Delay >= 0)
            {
                _state.Configuration.Delay = update.Body.Delay;
                _control.Delay = _state.Configuration.Delay;
            }

            _state.Configuration.PollingInterval = update.Body.PollingInterval;

            // Timeout is set, but not acted on
            if (update.Body.Timeout >= 0)
                _state.Configuration.Timeout = update.Body.Timeout;

            // Save the state for next time
            SaveState(_state);

            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }
        
        /// <summary>
        /// ConfigureDevices Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> ConfigureDevicesHandler(brick.ConfigureDevices update)
        {
            throw new NotImplementedException("ConfigureDevices is not possible");
        }

        /// <summary>
        /// Update Sensors Handler (Selective Update)
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> UpdateSensorsHandler(brick.UpdateSensors update)
        {
            bool changed = false;
            int i;

            // Iterate through all of the Inputs in the request
            foreach (brick.Device d in update.Body.Devices)
            {
                // Find the matching Hardware Id
                // This is inefficient, but it keeps the structures simple
                i = util.FindDeviceById(_state.Sensors, d.HardwareIdentifer);
                if (i < 0)
                    throw new ArgumentOutOfRangeException("Invalid Hardware Identifier: " + d.HardwareIdentifer);

                // Update according to the device type
                // NOTE: If it is not Analog or Digital, it is ignored!
                if (util.IsAnalogDevice(d))
                {
                    util.UpdateDeviceValue(_state.Sensors[i], d.Value);
                    changed = true;
                }
                if (util.IsDigitalDevice(d))
                {
                    // Update the state
                    util.UpdateDeviceState(_state.Sensors[i], d.State);
                    changed = true;
                }
            }

            // Send Notifications to subscribers if changed
            // NOTE: There are two options here --
            // Send all the sensors and let subscribers figure out what has changed
            // or send only the changes that we just received. In keeping with the
            // minimalist approach, we just send the update.
            if (changed)
                //SendNotification<brick.SensorsChanged>(_subMgrPort, new brick.SensorsChanged(_state.Sensors));
                SendNotification<brick.UpdateSensors>(_subMgrPort, update);

            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }
        
        /// <summary>
        /// PlayTone Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> PlayToneHandler(brick.PlayTone update)
        {
            // Turn on the buzzer and don't bother to wait for a response
            util.UpdateDeviceStateInList(_state.Actuators, (int)HwIds.Buzzer, true);
            _control.SpeakerState(true);

            // Set up a timer to turn the buzzer off
            Activate(Arbiter.Receive(false, TimeoutPort(update.Body.Duration),
                delegate(DateTime dt)
                {
                    // Turn off the buzzer
                    util.UpdateDeviceStateInList(_state.Actuators, (int)HwIds.Buzzer, false);
                    _control.SpeakerState(false);
                }
            ));

            // Send back the default response
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            yield break;
        }
        
        /// <summary>
        /// SetLEDs Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SetLEDsHandler(brick.SetLEDs update)
        {
            // Use the LEDMask to chop off invalid bits
            _state.LEDs = update.Body.States & LEDMask;

            // Update the Sensors list as well
            int mask = 1;
            for (int i = 0; i < LEDIds.Length; i++)
            {
                util.UpdateDeviceStateInList(_state.Actuators, LEDIds[i], ((mask & _state.LEDs) != 0));
                mask = mask << 1;
            }

            // Set the LEDs now but don't wait for the response
            _control.SetLEDs(_state.LEDs);

            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }
        
        /// <summary>
        /// GetSwitches Handler
        /// </summary>
        /// <param name="query"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetSwitchesHandler(brick.GetSwitches query)
        {
            // Should we poll or just return the current values?
            //_control.PollSwitches();
            //_state.Switches = _control.Switches;

            query.ResponsePort.Post(new brick.GetSwitchesResponse(_state.Switches));
            yield break;
        }
        
        /// <summary>
        /// GetSensors Handler
        /// </summary>
        /// <param name="query"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetSensorsHandler(brick.GetSensors query)
        {
            int n;

            // Create a new result list (which might end up empty!)
            List<brick.Device> list = new List<brick.Device>();

            foreach (brick.Device d in query.Body.Inputs)
            {
                // Look in the State for a matching Hardware Id
                n = util.FindDeviceById(_state.Sensors, d.HardwareIdentifer);
                if (n < 0)
                {
                    // No such device! Throw an exception and give up!
                    throw new ArgumentOutOfRangeException("No such hardware identifier: " + d.HardwareIdentifer);
                }
                else
                {
                    // Add the device to the result list
                    list.Add((brick.Device)_state.Sensors[n].Clone());
                }
            }
            // Return the list
            query.ResponsePort.Post(new brick.GetSensorsResponse(list));

            yield break;
        }
        
        /// <summary>
        /// SetActuators Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SetActuatorsHandler(brick.SetActuators update)
        {
            bool setLEDs = false;

            foreach (brick.Device d in update.Body.Outputs)
            {
                // Process Digital Outputs that are NOT Reserved
                if (util.IsDigitalDevice(d) && ((d.Type & brick.DeviceTypes.Reserved) == 0))
                {
                    if (d.Function == brick.DeviceFunctions.LED)
                    {
                        _state.LEDs = util.UpdateBitmask(_state.LEDs, LEDIds,
                                                    d.HardwareIdentifer, d.State);
                        util.UpdateDeviceStateInList(_state.Actuators, d.HardwareIdentifer, d.State);
                        setLEDs = true;
                    }
                    if (d.Function == brick.DeviceFunctions.Buzzer)
                    {
                        util.UpdateDeviceStateInList(_state.Actuators, d.HardwareIdentifer, d.State);
                        _control.SpeakerState(d.State);
                    }
                }

                // Process Analog Outputs that are NOT Reserved
                if (util.IsAnalogDevice(d) && ((d.Type & brick.DeviceTypes.Reserved) == 0))
                {
                    // There are no Analog outputs on the Hemisson!
                }
            }

            // Change LED states if necessary - this sets them all at once
            if (setLEDs)
                _control.SetLEDs(_state.LEDs);

            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }
        
        /// <summary>
        /// QueryDrivePower Handler
        /// </summary>
        /// <param name="query"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> QueryDrivePowerHandler(brick.QueryDrivePower query)
        {
            query.ResponsePort.Post(_state.DrivePower);
            yield break;
        }
        
        /// <summary>
        /// SetDrivePower Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SetDrivePowerHandler(drive.SetDrivePower update)
        {
            if (!_state.Connected)
            {
                throw new InvalidOperationException("Not connected to robot");
            }

            Port<drive.SetDrivePower> p = (Port<drive.SetDrivePower>)_mainPort[typeof(drive.SetDrivePower)];
            if (p.ItemCount > 10)
                LogError(LogGroups.Console, "SetDrivePower backlog: " + p.ItemCount);

            // Remove extra requests that might be banking up
            drive.SetDrivePower upd;
            int count = 0;
            while ((upd = _mainPort.Test<drive.SetDrivePower>()) != null)
            {
                // Acknowledge the update
                update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
                count++;
                update = upd;
            }

            if (count > 1)
                LogError(LogGroups.Console, "Skipped drive updates: " + count);

            bool changed = ((update.Body.LeftWheelPower != null &&
                _state.DrivePower.LeftPower != update.Body.LeftWheelPower)
                || (update.Body.RightWheelPower != null &&
                _state.DrivePower.RightPower != update.Body.RightWheelPower));

            if (update.Body.LeftWheelPower != null &&
                update.Body.LeftWheelPower >= -1.0 &&
                update.Body.LeftWheelPower <= 1.0)
                _state.DrivePower.LeftPower = update.Body.LeftWheelPower;

            if (update.Body.RightWheelPower != null &&
                update.Body.RightWheelPower >= -1.0 &&
                update.Body.RightWheelPower <= 1.0)
                _state.DrivePower.RightPower = update.Body.RightWheelPower;

            // If we are connected, send the Power to the robot
            if (_state.Connected)
            {
                yield return Arbiter.Receive(false,
                    _control.SetPower(_state.DrivePower.LeftPower, _state.DrivePower.RightPower),
                    delegate(byte[] resp)
                    { }
                );

                /*
                // Send Notifications to subscribers
                if (changed)
                {
                    //_subMgrPort.Post(new submgr.Submit(_state.DrivePower, DsspActions.UpdateRequest));
                    SendNotification<drive.SetDrivePower>(_subMgrPort, new drive.SetDrivePowerRequest(_state.DrivePower.LeftPower, _state.DrivePower.RightPower));
                }
                 */
            }
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }
        
        /// <summary>
        /// DriveDistance Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> DriveDistanceHandler(drive.DriveDistance update)
        {
            throw new NotImplementedException("DriveDistance is not implemented - Use Drive service");
        }
        
        /// <summary>
        /// RotateDegrees Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> RotateDegreesHandler(drive.RotateDegrees update)
        {
            throw new NotImplementedException("RotateDegrees is not implemented - Use Drive service");
        }

        #endregion

        #region Polling

        /// <summary>
        /// Set a Timer for Polling
        /// </summary>
        // NOTE: This does NOT guarantee an exact polling interval.
        // Instead it adds the polling interval to the current time.
        // There is only ever one receiver active at a time.
        private void SetPollTimer()
        {
            // Stop the timer if we are shutting down
            if (_shutdown)
                return;

            int wait = -1;
            // If the PollingInterval is negative, then polling is off
            if (_state.Configuration.PollingInterval >= 0 &&
                _state.Configuration.PollingInterval < _minimumPollingInterval)
                wait = _minimumPollingInterval;
            else
                wait = _state.Configuration.PollingInterval;

            if (wait >= 0)
                Activate(Arbiter.Receive(false, TimeoutPort(wait), PollingTimerHandler));
        }

        /// <summary>
        /// Reset the Timer after a timeout on a Poll
        /// </summary>
        /// <param name="dt"></param>
        private void SetPollTimer(DateTime dt)
        {
            LogError(LogGroups.Console, "Poll request did not respond");
            SetPollTimer();
        }

        /// <summary>
        /// Polling Timer Handler
        /// </summary>
        /// <param name="time"></param>
        private void PollingTimerHandler(DateTime time)
        {
            // Stop the timer if we are shutting down
            if (_shutdown)
                return;

            // Ignore timer if not connected, but keep it ticking over
            if (!_state.Connected)
            {
                SetPollTimer();
                return;
            }

            // Is Polling turned off?
            if (_state.Configuration.PollingInterval < 0)
            {
                // Set the timer, but don't do anything
                SetPollTimer();
                return;
            }

            // Get Sensors
            Port<byte[]> response;
            response = _control.Poll();

            Activate(Arbiter.Choice(
                Arbiter.Receive<byte[]>(false, response, ProcessPollResult),
                // Make sure that we don't hang here forever!
                Arbiter.Receive<DateTime>(false, TimeoutPort(1000), SetPollTimer)));
        }

        /// <summary>
        /// Handle results from a Poll and reset Timer
        /// </summary>
        /// <param name="result"></param>
        private void ProcessPollResult(byte[] result)
        {
            // NOTE: The result should always be 22 bytes long or there is
            // something wrong
            if (result.Length == 23)
            {
                bool changed = false;

                // Process the response
                // Format should be '&' followed by 22 bytes of data:
                // Motors:
                // PwmMotLeft,                  1
                // PwmMotRight,                 2
                // IR range sensrors:
                // IR_Proximity[ Front ],       3
                // IR_Proximity[ FrontRight ],  4
                // IR_Proximity[ FrontLeft ],   5
                // IR_Proximity[ Right],        6
                // IR_Proximity[ Left],         7
                // IR_Proximity[ Rear ],        8
                // IR_Proximity[ GroundRight ], 9
                // IR_Proximity[ GroundLeft ],  10
                // Light settings (we do not expose these):
                // IR_Light[ Front ],           11
                // IR_Light[ FrontRight ],      12
                // IR_Light[ FrontLeft ],       13
                // IR_Light[ Right],            14
                // IR_Light[ Left],             15
                // IR_Light[ Rear ],            16
                // IR_Light[ GroundRight ],     17
                // IR_Light[ GroundLeft ],      18
                // Others:
                // Switches,                    19
                // TV_DATA,                     20
                // HEMIOS_VERSION,              21
                // HEMIOS_REVISION              22
                // and then CR LF (which are not returned)
                if (result[0] == (byte)'&')
                {
                    // NOTE: We do not send updates on switch changes!
                    _state.Switches = (int) result[19];

                    // Process all the sensors
                    foreach (brick.Device d in _state.Sensors)
                    {
                        switch ((HwIds) d.HardwareIdentifer)
                        {
                            case HwIds.IRFront:
                                util.UpdateDeviceValue(d, (double)result[3]);
                                changed = true;
                                break;
                            case HwIds.IRFrontRight:
                                util.UpdateDeviceValue(d, (double)result[4]);
                                changed = true;
                                break;
                            case HwIds.IRFrontLeft:
                                util.UpdateDeviceValue(d, (double)result[5]);
                                changed = true;
                                break;
                            case HwIds.IRRight:
                                util.UpdateDeviceValue(d, (double)result[6]);
                                changed = true;
                                break;
                            case HwIds.IRLeft:
                                util.UpdateDeviceValue(d, (double)result[7]);
                                changed = true;
                                break;
                            case HwIds.IRRear:
                                util.UpdateDeviceValue(d, (double)result[8]);
                                changed = true;
                                break;
                            case HwIds.IRGroundRight:
                                util.UpdateDeviceValue(d, (double)result[9]);
                                changed = true;
                                break;
                            case HwIds.IRGroundLeft:
                                util.UpdateDeviceValue(d, (double)result[10]);
                                changed = true;
                                break;
                            case HwIds.Switch1:
                                util.UpdateDeviceState(d, ((_state.Switches & 1) != 0));
                                break;
                            case HwIds.Switch2:
                                util.UpdateDeviceState(d, ((_state.Switches & 2) != 0));
                                break;
                            case HwIds.Switch3:
                                util.UpdateDeviceState(d, ((_state.Switches & 4) != 0));
                                break;
                            case HwIds.Switch4:
                                util.UpdateDeviceState(d, ((_state.Switches & 8) != 0));
                                break;
                            default:
                                break;
                        }
                    }

                    // Only send notifications on changes
                    if (changed)
                        SendNotification<brick.UpdateSensors>(_subMgrPort, new brick.UpdateSensorsRequest(_state.Sensors));

                }
            }
            else
            {
                LogError(LogGroups.Console, "Invalid response to Poll");
            }

            // Finally, set the timer again
            SetPollTimer();
        }

        #endregion

        #region Robot Connection

        /// <summary>
        /// Close the connection to the Robot
        /// </summary>
        private void Disconnect()
        {
            if (_state.Connected)
            {
                _control.Close();
                _state.Connected = false;
            }
        }

        /// <summary>
        /// Connect to the Robot
        /// </summary>
        /// <returns></returns>
        private bool ConnectToRobot()
        {
            string errorMessage;

            if (_state.Connected)
                return true;

            // Let user know what is happening
            // NOTE: Cannot use LogInfo here because this module is not derived
            // from the service base class!
            LogInfo(LogGroups.Console, "Connecting ...");

            // Attempt to connect to the robot
            _state.Connected = _control.Connect(_state.Configuration, out errorMessage);
            if (!_state.Connected && !string.IsNullOrEmpty(errorMessage))
            {
                // Display the error message and give up
                LogError(LogGroups.Activation, errorMessage);
                return false;
            }

            if (_control.WaitForConnect(_state.Firmware))
            {
                // Display the version for user to see
                LogInfo(LogGroups.Console, "Firmware Version: " + _state.Firmware.VersionString);

                // Indicate that connection was successful
                LogInfo(LogGroups.Console, "... Connected");

            }
            else
            {
                // Display an error message
                LogError(LogGroups.Activation, "Connection to robot failed.\nPlease fix the problem and re-start the service.\n");
                _state.Connected = false;
            }

            return _state.Connected;
        }

        #endregion

    }
}
